#include "PID.h"

void PID_Init(pid_str* str)
{
    str->kp = 0;
    str->ki = 0;
    str->kd = 0;
    str->imax = 0;

    str->out_p = 0;
    str->out_i = 0;
    str->out_d = 0;
    str->out = 0;

    str->last_error = 0;
    str->last_derivative = 0;
    str->integrator = 0;
    str->target = 0;
}

void PID_Set(pid_str* str,float kp,float ki,float kd,float imax)
{
    str->kp = kp;
    str->ki = ki;
    str->kd = kd;
    str->imax = imax;
}

void PID_Set_Target(pid_str* str,float target)
{
    str->target = target;
}

float PID_imax(float data,float low,float high)
{
    if (data <= low) {
        return low;
    }
    else if (data >= high) {
        return high;
    }
    else {
        return data;
    }
}

float PID_Place(pid_str* str,float actual_val)
{
    float error = str->target - actual_val;
    str->integrator += error;
    str->integrator = PID_imax(str->integrator,-str->imax,str->imax);

    str->out_p = str->kp*error;
    str->out_i = str->ki*str->integrator;
    str->out_d = str->kd*(error - str->last_error);
    str->out = str->out_p + str->out_i + str->out_d;

    str->last_error = error;

    return str->out;
}

float PID_place_I_Div(pid_str* str,float actual_val,float error_limit)
{
    float error = str->target - actual_val;
    str->integrator += error;

    if (error > error_limit || error < -error_limit) {
        str->integrator = 0;
    }

    str->out_p = str->kp*error;
    str->out_i = str->ki*str->integrator;
    str->out_d = str->kd*(error - str->last_error);
    str->out = str->out_p + str->out_i + str->out_d; 

    str->last_error = error;

    return str->out;
}

float PID_Increase(pid_str* str,float actual_val)
{
    float error = str->target - actual_val;

    str->out_p = str->kp*(error - actual_val);
    str->out_i = str->ki*error;
    str->out_d = str->kd*(error - actual_val - str->last_derivative);
    str->out = str->out_p + str->out_i + str->out_d;

    str->last_derivative = str->last_error;
    str->last_error = error;

    return str->out;
}